/******************************************************************************
/// @file    err_task.c
/// @author  langgo
/// @copyright Copyright (c) 2017 <dji-innovations, Corp. RM Dept.>
/// @license MIT License
/// Permission is hereby granted, free of charge, to any person obtaining a copy
/// of this software and associated documentation files (the "Software"), to deal
/// in the Software without restriction,including without limitation the rights
/// to use, copy, modify, merge, publish, distribute, sublicense,and/or sell
/// copies of the Software, and to permit persons to whom the Software is furnished
/// to do so,subject to the following conditions:
///
/// The above copyright notice and this permission notice shall be included in
/// all copies or substantial portions of the Software.
///
/// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
/// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
/// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
/// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
/// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
/// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
/// THE SOFTWARE.
*******************************************************************************/

#include "error_task.h"
#include "cmsis_os.h"
#include "led.h"
#include "mytype.h"
#include "stm32f4xx_hal.h"

#define BEEP_ON (TIM3->CCR1 = 599)
#define BEEP_OFF (TIM3->CCR1 = 0)

#define BEEP_ARR (TIM3->ARR)
#define BEEP_CH (TIM3->CCR1)

/* global variables --------------------------*/
global_rx_err_t g_rx_err;

void* err_str[ErrorListLength] = {
    "none",
    "Gyro",
    "mot1",
    "mot2",
    "mot3",
    "mot4",
    "CuSa",
    "Dbus",
    "Judg",
    "PitX",
    "YawX",
    "Poke",
};

const u16 note_arr_tab[] = {
    382, 341, 303, 286, 255, 227, 202, //低音7
    191, 170, 152, 143, 128, 114, 101, //中音7
    96, 85, 76, 72, 64, 57, 51, //高音7
};

//sing 100ms
void sing(u8 note)
{
    if (Silent == note)
        BEEP_CH = 0;
    else
    {
        BEEP_ARR = note_arr_tab[note];
        BEEP_CH  = note_arr_tab[note] / 2;
    }
}

void beep_sing(int s)
{

    BEEP_ARR = 999;
    if (s & 0x80000000)
        LED_R_ON; //BEEP_ON;
    else
        LED_R_OFF; //BEEP_OFF;
}

//这些函数本来要作为错误处理的handler， 由函数指针调用，现在还没写
//static void ErrorDealParamInit(void);
//static void UndPanGyroxyRxErrorDeal(void);
//static void minPCRxErrorDeal(void);
//static void CanYawRxErrorDeal(void);
//static void CanPitchRxErrorDeal(void);
//static void RCRxErrorDeal(void);
//static void scanErrorFlag(void);

void global_err_detector_init(void)
{
    g_rx_err.now = NULL;

    g_rx_err.list[DbusTOE].err_exist = 0;
    //	gRxErr.err_list[DbusTOE].err_id = DbusTOE;	//seems no use
    g_rx_err.list[DbusTOE].warn_pri    = 10; //max !
    g_rx_err.list[DbusTOE].beep_mask   = Bi_Bi;
    g_rx_err.list[DbusTOE].set_timeout = 100; //ms
    g_rx_err.list[DbusTOE].delta_time  = 0;
    g_rx_err.list[DbusTOE].last_time   = 0x00;
    g_rx_err.list[DbusTOE].enable      = 1;

    for (int i = 0; i < 4; i++)
    {
        g_rx_err.list[ChassisMoto1TOE + i].err_exist = 0;
        //		gRxErr.err_list[ChassisMoto1TOE+i].err_id = ChassisMoto1TOE;
        g_rx_err.list[ChassisMoto1TOE + i].warn_pri    = 6 + i; //6,7,8,9
        g_rx_err.list[ChassisMoto1TOE + i].beep_mask   = Biii_Biii_Bi;
        g_rx_err.list[ChassisMoto1TOE + i].set_timeout = 200;
        g_rx_err.list[ChassisMoto1TOE + i].delta_time  = 0;
        g_rx_err.list[ChassisMoto1TOE + i].last_time   = 0x00;
        g_rx_err.list[ChassisMoto1TOE + i].enable      = 1;
    }
    //gRxErr.err_list[ChassisMoto1TOE].enable = 1;

    g_rx_err.list[ChassisGyroTOE].err_exist = 0;
    //gRxErr.err_list[ChassisGyroTOE].err_id = ChassisGyroTOE;
    g_rx_err.list[ChassisGyroTOE].warn_pri    = 5;
    g_rx_err.list[ChassisGyroTOE].beep_mask   = Bi_Biii_Bi;
    g_rx_err.list[ChassisGyroTOE].set_timeout = 10;
    g_rx_err.list[ChassisGyroTOE].delta_time  = 0;
    g_rx_err.list[ChassisGyroTOE].last_time   = 0x00;
    g_rx_err.list[ChassisGyroTOE].enable      = 1;

    g_rx_err.list[CurrentSampleTOE].err_exist = 0;
    //	gRxErr.err_list[CurrentSampleTOE].err_id = CurrentSampleTOE;
    g_rx_err.list[CurrentSampleTOE].warn_pri    = 4;
    g_rx_err.list[CurrentSampleTOE].beep_mask   = Bi_Bi_Bi;
    g_rx_err.list[CurrentSampleTOE].set_timeout = 200;
    g_rx_err.list[CurrentSampleTOE].delta_time  = 0;
    g_rx_err.list[CurrentSampleTOE].last_time   = 0x00;
    //gRxErr.err_list[CurrentSampleTOE].enable = 1;

    g_rx_err.list[GimbalPitTOE].err_exist   = 0;
    g_rx_err.list[GimbalPitTOE].warn_pri    = 8;
    g_rx_err.list[GimbalPitTOE].beep_mask   = Bi_Bi_Bi;
    g_rx_err.list[GimbalPitTOE].set_timeout = 200;
    g_rx_err.list[GimbalPitTOE].delta_time  = 0;
    g_rx_err.list[GimbalPitTOE].last_time   = 0x00;
    g_rx_err.list[GimbalPitTOE].enable      = 1;

    g_rx_err.list[GimbalYawTOE].err_exist   = 0;
    g_rx_err.list[GimbalYawTOE].warn_pri    = 8;
    g_rx_err.list[GimbalYawTOE].beep_mask   = Bi_Bi_Bi;
    g_rx_err.list[GimbalYawTOE].set_timeout = 200;
    g_rx_err.list[GimbalYawTOE].delta_time  = 0;
    g_rx_err.list[GimbalYawTOE].last_time   = 0x00;
    g_rx_err.list[GimbalYawTOE].enable      = 1;

    g_rx_err.list[PokeMotoTOE].err_exist   = 0;
    g_rx_err.list[PokeMotoTOE].warn_pri    = 2;
    g_rx_err.list[PokeMotoTOE].beep_mask   = Bi_Bi_Bi;
    g_rx_err.list[PokeMotoTOE].set_timeout = 200;
    g_rx_err.list[PokeMotoTOE].delta_time  = 0;
    g_rx_err.list[PokeMotoTOE].last_time   = 0x00;
    g_rx_err.list[PokeMotoTOE].enable      = 1;

    g_rx_err.str = err_str[NoTimeOutErr];
}

//把这个函数插入到中断回调或者触发式中断同步函数中如CAN rx callback， usart。。。
void err_detector_hook(int err_id)
{

    if (g_rx_err.list[err_id].enable)
        g_rx_err.list[err_id].last_time = HAL_GetTick();
}

void power_up_music()
{

    sing(Fa4M);
    osDelay(250);
    sing(Mi3M);
    osDelay(250);
    sing(Re2M);
    osDelay(250);
    sing(Do1M);
    osDelay(250);
    sing(Silent);
    osDelay(100);

    sing(Do1M);
    osDelay(100);
    sing(Silent);
    osDelay(50);
    sing(Do1M);
    osDelay(100);
    sing(Re2M);
    osDelay(100);
    sing(La6L);
    osDelay(200);
    sing(So5L);
    osDelay(200);
    sing(La6L);
    osDelay(200);
    sing(Silent);
}

//不能这样写！
void err_music()
{

    sing(Re2M);
    osDelay(400);
    sing(Fa4M);
    osDelay(400);
    sing(So5M);
    osDelay(300);
    sing(Silent);
    osDelay(100);
    sing(So5M);
    osDelay(300);
    sing(La6M);
    osDelay(300);
    sing(Silent);
    osDelay(100);
    sing(La6M);
    osDelay(300);
    sing(Silent);
    osDelay(500);
}

void err_music2()
{

    sing(La6L);
    osDelay(200);
    sing(Do1M);
    osDelay(200);
    sing(Re2M);
    osDelay(200);
    sing(Silent);
    osDelay(100);

    sing(So5M);
    osDelay(500);
    sing(Mi3M);
    osDelay(500);
    sing(Fa4M);
    osDelay(200);

    sing(Mi3M);
    osDelay(200);
    sing(Do1M);
    osDelay(200);
    sing(Re2M);
    osDelay(400);
}

void err_detector_task(void const* argument)
{
    global_err_detector_init();
    osDelay(100);
    //power_up_music();
    //	err_music2();
    for (;;)
    {
        u32 systime      = HAL_GetTick();
        int max_priority = 0;
        int err_cnt      = 0;
        for (int id = 0; id < ErrorListLength; id++)
        {
            g_rx_err.list[id].delta_time = HAL_GetTick() - g_rx_err.list[id].last_time;
            if (g_rx_err.list[id].enable && g_rx_err.list[id].delta_time > g_rx_err.list[id].set_timeout)
            {
                g_rx_err.list[id].err_exist = 1; //now err exist!!
                err_cnt++;
                g_rx_err.id = (timeout_id_e)id;
                if (g_rx_err.list[id].warn_pri > max_priority)
                {
                    max_priority   = g_rx_err.list[id].warn_pri;
                    g_rx_err.now = &(g_rx_err.list[id]);
                    g_rx_err.str     = err_str[id];
                }
                //update max
            }
            else
            {
                g_rx_err.list[id].err_exist = 0;
            }
        }

        if (!err_cnt) //all scan no error, should clear err pointer!!!
            g_rx_err.now = NULL;

        int beep_seq = 0;
        if (g_rx_err.now != NULL)
        {
            beep_seq = g_rx_err.now->beep_mask;
            for (int i = 0; i < 32; i++)
            {
                beep_sing(beep_seq << i);
                osDelay(100);
            }
            LED_G_OFF;
        }
        else
        {
            beep_sing(0); //必须写！！不然导致有时候一直长响
            LED_G_ON;
        }

        //clear err_now every 100ms

        //clr_err_cnt = 0;
        g_rx_err.str = err_str[NoTimeOutErr];
        osDelay(50);
    }
}

//C1 C2 C3
void set_led(int led)
{
    LED_PORT->ODR &= ~(7 << 1); //因为1是灭 所以取反
    LED_PORT->ODR |= ~(led << 1);
}

void err_deadloop()
{

    set_led(LED_R | LED_B);
    while (1)
    {
        set_led(LED_R | LED_B);
        HAL_Delay(300);
        set_led(LED_R | LED_B);
        HAL_Delay(1000);
    }
}
